﻿/* Copyright 2007-2008 dnAnalytics Project.
 *
 * Contributors to this file:
 * Nat Luengnaruemitchai 
 * Marcus Cuda
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * * Redistributions of source code must retain the above copyright notice, this 
 *   list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice, 
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * * Neither the name of the dnAnalytics Project nor the names of its contributors
 *   may be used to endorse or promote products derived from this software without
 *   specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

namespace dnAnalytics.LinearAlgebra.Decomposition
{
    //port of the public domain Jampack QR
    internal class DenseHouseholder : AbstractHouseholder
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="DenseHouseholder"/> class.
        /// </summary>
        /// <param name="matrix">The matrix to decompose.</param>
        public DenseHouseholder(Matrix matrix) : base(matrix)
        {
        }

        /// <summary>
        /// Computes the decomposition.
        /// </summary>
        protected override void DoCompute()
        {
            mQ = new DenseMatrix(mRows);
            double[] q = ((DenseMatrix) mQ).Data;
            double[] r = ((DenseMatrix) mR).Data;

            for (int i = 0; i < mRows; i++)
            {
                q[i*mRows + i] = 1.0;
            }
            int minmn = System.Math.Min(mRows, mColumns);

            double[][] u = new double[minmn][];
            for (int i = 0; i < minmn; i++)
            {
                u[i] = GenerateColumn(r, mRows, i, mRows - 1, i);
                UA(u[i], r, mRows, i, mRows - 1, i + 1, mColumns - 1);
            }

            for (int i = minmn - 1; i >= 0; i--)
            {
                UA(u[i], q, mRows, i, mRows - 1, i, mRows - 1);
            }
        }

        protected override void DoSolve(Matrix b, Matrix result)
        {
            if (b is DenseMatrix)
            {
                DenseSolve(mRows, mColumns, b.Columns, ((DenseMatrix)mQ).Data, ((DenseMatrix)mR).Data, ((DenseMatrix)b).Data);
            }
            else 
            {
                Matrix tmp = new DenseMatrix(b);
                DenseSolve(mRows, mColumns, b.Columns, ((DenseMatrix)mQ).Data, ((DenseMatrix)mR).Data, ((DenseMatrix)tmp).Data);
                tmp.CopyTo(b);
            }

            //DenseSolve(mRows, mColumns, b.Columns, ((DenseMatrix) mQ).Data, ((DenseMatrix) mR).Data, ((DenseMatrix) b).Data);
            for (int i = 0; i < mR.Columns; i++)
            {
                for (int j = 0; j < b.Columns; j++)
                {
                    result.ValueAt(i, j, b.ValueAt(i, j));
                }
            }
        }

        protected override void DoSolve(Vector b, Vector result)
        {
            DenseSolve(mRows, mColumns, 1, ((DenseMatrix) mQ).Data, ((DenseMatrix) mR).Data, ((DenseVector) b).Data);
            for (int i = 0; i < mR.Columns; i++)
            {
                result[i] = b[i];
            }
        }

        private static void DenseSolve(int m, int n, int bn, double[] q, double[] r, double[] b)
        {
            // Compute Y = transpose(Q)*B
            double[] column = new double[m];
            for (int j = 0; j < bn; j++)
            {
                int jm = j*m;
                for (int k = 0; k < m; k++)
                {
                    column[k] = b[jm + k];
                }
                for (int i = 0; i < m; i++)
                {
                    double s = 0;
                    int im = i*m;
                    for (int k = 0; k < m; k++)
                    {
                        s += q[im + k]*column[k];
                    }
                    b[jm + i] = s;
                }
            }

            // Solve R*X = Y;
            for (int k = n - 1; k >= 0; k--)
            {
                int km = k*m;
                for (int j = 0; j < bn; j++)
                {
                    b[j*m + k] /= r[km + k];
                }
                for (int i = 0; i < k; i++)
                {
                    for (int j = 0; j < bn; j++)
                    {
                        int jm = j*m;
                        b[jm + i] -= b[jm + k]*r[km + i];
                    }
                }
            }
        }

        private static double[] GenerateColumn(double[] A, int m, int r1, int r2, int c)
        {
            int ru = r2 - r1 + 1;
            int tmp = c*m;
            int index = tmp + r1;
            double[] u = new double[ru];

            for (int i = r1; i <= r2; i++)
            {
                u[i - r1] = A[tmp + i];
                A[tmp + i] = 0.0;
            }

            double norm = Norm(u);

            if (r1 == r2 || norm == 0)
            {
                A[index] = -u[0];
                u[0] = System.Math.Sqrt(2.0);
                return u;
            }

            double scale = 1.0/norm;
            if (u[0] < 0.0)
            {
                scale *= -1.0;
            }

            A[index] = -1.0/scale;

            for (int i = 0; i < ru; i++)
            {
                u[i] *= scale;
            }

            u[0] += 1.0;
            double s = System.Math.Sqrt(1.0/u[0]);

            for (int i = 0; i < ru; i++)
            {
                u[i] *= s;
            }
            return u;
        }

        private static void UA(double[] u, double[] A, int m, int r1, int r2, int c1, int c2, double[] v)
        {
            if (r2 < r1 || c2 < c1)
            {
                return;
            }

            for (int j = c1; j <= c2; j++)
            {
                v[j - c1] = 0.0;
            }

            for (int i = r1; i <= r2; i++)
            {
                for (int j = c1; j <= c2; j++)
                {
                    v[j - c1] = v[j - c1] + u[i - r1]*A[j*m + i];
                }
            }

            for (int i = r1; i <= r2; i++)
            {
                for (int j = c1; j <= c2; j++)
                {
                    A[j*m + i] = A[j*m + i] - u[i - r1]*v[j - c1];
                }
            }
        }

        private static void UA(double[] u, double[] A, int m, int r1, int r2, int c1, int c2)
        {
            if (c1 > c2)
            {
                return;
            }
            UA(u, A, m, r1, r2, c1, c2, new double[c2 - c1 + 1]);
        }
    }
}
